﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using Emgu.CV;
using Emgu.CV.Structure;
using Emgu.Util;
using Emgu.CV.CvEnum;
using System.Drawing.Drawing2D;
using System.Xml;
using System.Xml.Serialization;
using System.Runtime.InteropServices;

namespace Projekt_naczepa
{
    public partial class Form1 : Form
    {
        private int xIndexUpper;
        private int yIndexUpper;
        private int xIndexBottom;
        Capture captureCam;
        Image<Bgr, Byte> imgOriginal;
        Image<Bgr, Byte> imgFoundLineAndPoint;
        Image<Bgr, Byte> imgVertical;
        bool binaryProcessed;
        int[] countPixelLineInColumns;
        int[] countPixelLineInRows;
        Bitmap myBmp;
        Bitmap myBmpOriginal;
        Point pUpper;
        Point pBottom;
        VideoWriter myVideo;
        bool recorder;
        Image<Bgr, byte> korekta;
        private Matrix<float> x_mat;// = new Matrix<float>(2592, 1944);
        private Matrix<float> y_mat;// = new Matrix<float>(2592, 1944);

        public Form1()
        {
            InitializeComponent();
            this.binaryProcessed = true;
        }

        private void Form1_Load(object sender, EventArgs e)
        {
            countPixelLineInColumns = new int[imageBoxOriginal.Width];
            countPixelLineInRows = new int[imageBoxOriginal.Height];
            pUpper = new Point();
            pBottom = new Point();
            myBmp = new Bitmap(imageBoxOriginal.Width, imageBoxOriginal.Height);
            myBmpOriginal = new Bitmap(imageBoxOriginal.Width, imageBoxOriginal.Height);
            imgFoundLineAndPoint = new Image<Bgr, byte>(imageBoxProcessed.Width, imageBoxProcessed.Height);
            imgOriginal = new Image<Bgr, byte>("C:/Users/Rafał/Desktop/tir/test1.jpg");  //imgOriginal = new Image<Bgr, byte>(imageBoxOriginal.Width, imageBoxOriginal.Height);
            imgVertical = new Image<Bgr, byte>(imageBoxOriginal.Width, imageBoxOriginal.Height);
    //        captureCam = new Capture();
            korekta = new Image<Bgr, byte>(imgOriginal.Size);
            System.Console.WriteLine("Size: x=" + imgOriginal.Width.ToString() + ", y = " + imgOriginal.Height.ToString());
            x_mat = new Matrix<float>(imgOriginal.Size);
            y_mat = new Matrix<float>(imgOriginal.Size);

            myVideo = new VideoWriter("calibration2.avi",5, 1296, 972, true);
            recorder = true;
            //readSerializedFile();
  //         Application.Idle += ProcessFrame;
  //          Application.Idle += button1_Click;
  //          Application.Idle += bFindLine_Click;
            //tmp_undis();

            //imageBoxOriginal.Image = korekta;
            //ProcessFrame();
            button1_Click(null, null);
        }

        private void ProcessFrame()//object sender, EventArgs e)
        {
            //imgOriginal = captureCam.QueryFrame();
            var rot = new Image<Bgr, byte>(imgOriginal.Height, imgOriginal.Width);
            Emgu.CV.CvInvoke.cvTranspose(imgOriginal.Ptr,rot.Ptr);
            rot._Flip(Emgu.CV.CvEnum.FLIP.VERTICAL);
            imageBoxOriginal.Image = rot;
            imgVertical = rot;//.ToBitmap(imageBoxOriginal.Width,imageBoxOriginal.Height);
            
        }

        private void button1_Click(object sender, EventArgs e)
        {
          //  if (NR_BOX.Value<29) NR_BOX.Value++;
            string nazwa = "C:/Users/Rafał/Desktop/tir/test" + NR_BOX.Value.ToString() + ".jpg";

            imgOriginal = new Image<Bgr, byte>(nazwa);
            tmp_undis();
            ProcessFrame();
            myBmp = imgVertical.ToBitmap(imageBoxOriginal.Width, imageBoxOriginal.Height); //imageBoxOriginal.Image.Bitmap;//captureCam.QueryFrame().ToBitmap(imageBoxOriginal.Width, imageBoxOriginal.Height);
            myBmpOriginal = imgVertical.ToBitmap(imageBoxOriginal.Width, imageBoxOriginal.Height);//captureCam.QueryFrame().ToBitmap(imageBoxOriginal.Width, imageBoxOriginal.Height);
            
            for (int i = 0; i < myBmp.Width; i++)
            {
                for (int j = 0; j < myBmp.Height; j++)
                {
                    if (myBmp.GetPixel(i, j).R > myBmp.GetPixel(i, j).G && myBmp.GetPixel(i, j).R > myBmp.GetPixel(i, j).B && myBmp.GetPixel(i, j).R > tbRed.Value && myBmp.GetPixel(i,j).G + myBmp.GetPixel(i,j).B < TB_GB.Value)
                    {
                        myBmp.SetPixel(i, j, Color.White);
                       countPixelLineInColumns[i]++;                    //Zwiększamy liczbę pikseli w danej kolumnie odpowiadających zadanym warunkom
                    }
                    else
                    {
                        myBmp.SetPixel(i, j, Color.Black);
                    }
                }
            }
          
//            MCvMat mat_1;

            

            pictureBox1.Image = myBmp;
            bFindLine_Click(sender, e);
        }

        
        private void bFindLine_Click(object sender, EventArgs e)
        {          
            getIndexColumnLine();
            getIndexRowLine();
            pUpper.X = yIndexUpper;
            pUpper.Y = xIndexUpper;
            pBottom.X = yIndexUpper;
            pBottom.Y = xIndexBottom; 
            imgFoundLineAndPoint = new Image<Bgr, Byte>(myBmpOriginal);
 //           imgFoundLineAndPoint.Draw(new LineSegment2D(pUpper,pBottom), new Bgr(Color.Green),2);
           
           
            if (xIndexUpper > 15 && xIndexBottom < (countPixelLineInRows.Length - 15))
            {
                imgFoundLineAndPoint.Draw(new CircleF(pUpper, 3), new Bgr(Color.Blue), 2);
                imgFoundLineAndPoint.Draw(new CircleF(pBottom, 3), new Bgr(Color.Green), 2);
                //nudDistance1.Value = xIndexUpper;
                calculateDistance();
                calculateDistanceFromTheGround();
            }
            else if (xIndexUpper > 15)
            {
                imgFoundLineAndPoint.Draw(new CircleF(pUpper, 3), new Bgr(Color.Blue), 2);
    //            nudDistance1.Value = xIndexUpper;
                calculateDistance();
                calculateDistanceFromTheGround();
            }
            else if (xIndexBottom < (countPixelLineInRows.Length - 15))
            {
                imgFoundLineAndPoint.Draw(new CircleF(pBottom, 3), new Bgr(Color.Green), 2);
 //               nudDistance1.Value = xIndexBottom;
            }
            imageBoxProcessed.Image = imgFoundLineAndPoint;
            setZero();
        }

        private void getIndexColumnLine()            //Zwraca index kolumny, która zawiera njwięcej szukanych pikseli (linii lasera)
        {
            int max = countPixelLineInColumns.Max();
            for (int i = 0; i < countPixelLineInColumns.Length; i++)
            {
                if (countPixelLineInColumns[i] == max)
                    yIndexUpper = i;
            }
        }

        private void getIndexRowLine()
        {
            xIndexUpper = 0;
            xIndexBottom = 0;
            if (yIndexUpper > 10 && (yIndexUpper + 10) < countPixelLineInColumns.Length)
            {
                for (int i = yIndexUpper - 10; i < yIndexUpper + 10; i++)
                {
                    for (int j = 0; j < myBmp.Height; j++)
                    {                      
                        if (myBmp.GetPixel(i, j).R == 255 && myBmp.GetPixel(i, j).G == 255 && myBmp.GetPixel(i, j).B == 255)
                        {
                            countPixelLineInRows[j]++;
                        }
                    }
                }           
                int min = countPixelLineInRows.Min();
                if (min > 0)
                {
                    xIndexUpper = 0;
                    xIndexBottom = 0;
                }
                else
                {
                    for (int k = 15; k < countPixelLineInRows.Length - 25; k++)
                    {
                        if (countPixelLineInRows[k] != 0)
                        {
                            myBmpOriginal.SetPixel(yIndexUpper, k, Color.Green);
                        }
                        if (countPixelLineInRows[k] == min && countPixelLineInRows[k + 1] > min && countPixelLineInRows[k - 1] == min
                            && countPixelLineInRows[k - 5] == min && countPixelLineInRows[k - 10] == min && countPixelLineInRows[k - 15] == min)
                        {
                            xIndexUpper = k;
                            continue;
                        }
                        if (countPixelLineInRows[k] == min && countPixelLineInRows[k - 1] > min && countPixelLineInRows[k + 1] == min
                            && countPixelLineInRows[k + 5] == min && countPixelLineInRows[k + 10] == min && countPixelLineInRows[k + 15] == min && countPixelLineInRows[k + 25] == min)
                        {
                            xIndexBottom = k;
                            break;
                        }
                    }
                }
            }
        }

        private void calculateDistance() 
        { 
            nudNumberOfPixels.Value = xIndexUpper;
            //if (xIndexUpper > 300)
            //{
                //nudDistance1.Value = Convert.ToInt32(xIndexUpper * (Math.Pow(0.00798840, 0.00178569830 * xIndexUpper - 0.688454859) + 0.25205575));
            //}
            //else
            //{
            int static_error = 25+50;
            
            
            nudDistance1.Value = static_error + Convert.ToInt32(xIndexUpper * (Math.Pow(6.475040481646401e-001, 5.662744863873603e-002 * xIndexUpper - 1.126100329494452e+001) + 2.103384453745234e+000));
            nudDistance1.Value = (int)((double)nudDistance1.Value * 0.97);
            //}
        }

        private void calculateDistanceFromTheGround() 
        {
            double distance = Convert.ToDouble(nudDistance1.Value);// xIndexUpper * (Math.Pow(6.475040481646401e-001, 5.662744863873603e-002 * xIndexUpper - 1.126100329494452e+001) + 2.103384453745234e+000);
         /*   if (xIndexUpper > 300)
            {
                //8.198312626549649e-001    2.038421379423239e-002   -3.729700679503889e+000    9.053137930159123e-002

                double alfa = 8.198312626549649e-001 * Math.Log10((2.038421379423239e-002 * distance) -3.729700679503889e+000) +9.053137930159123e-002;
                double tangens = Math.Tan(Math.PI - alfa);
                nudDisttanceFromGround.Value = Convert.ToInt32((tangens * xIndexBottom) + 980 - (tangens * imageBoxOriginal.Height*0.5));
 /*               double distance = xIndexUpper * (Math.Pow(0.00798840, 0.00178569830 * xIndexUpper - 0.688454859) + 0.25205575);
                double alfa = 0.97683317 * Math.Log10((0.00445430 * distance) + 1.5249706) + 0.2110058;
                double tangens = Math.Tan(Math.PI - alfa);
                nudDisttanceFromGround.Value = Convert.ToInt32((tangens * xIndexBottom) + 980 - (tangens * imageBoxOriginal.Height));
  */        /*  }
            else*/

            {
                double alfa = 8.198312626549649e-001 * Math.Log10((2.038421379423239e-002 * distance) - 3.729700679503889e+000) + 9.053137930159123e-002;
                //double alfa = 0.92 * Math.Log10((0.89 * distance) + 250) - 1.835;
                double tangens = Math.Tan(Math.PI - alfa);
                nudDisttanceFromGround.Value = Convert.ToInt32((tangens * xIndexBottom) + 840 - (tangens * imageBoxOriginal.Height*0.5));
 /*               double distance = xIndexUpper * (Math.Pow(0.136435233, 0.00686835 * xIndexUpper - 2.092847758) + 1.27735868);
                double alfa = 0.97683317 * Math.Log10((0.00445430 * distance) + 1.5249706) + 0.2110058;
                double tangens = Math.Tan(Math.PI - alfa);
                nudDisttanceFromGround.Value = Convert.ToInt32((tangens * xIndexBottom) + 980 - (tangens * imageBoxOriginal.Height));
 */           }
        }

        private void setZero() 
        {
            int i;
            for (i = 0; i < countPixelLineInColumns.Length; i++)
            {
                countPixelLineInColumns[i] = 0;
            }
            for (i = 0; i < countPixelLineInRows.Length - 15; i++)
            {
                countPixelLineInRows[i] = 0;
            }
            
        }

        private void recordFilm(object sender, EventArgs e)
        {
            imgOriginal = captureCam.QueryFrame();
            myVideo.WriteFrame<Bgr, Byte>(imgOriginal);
        }

        private void bRecorder_Click(object sender, EventArgs e)
        {
            if (recorder)
            {
                Application.Idle += recordFilm;
                bRecorder.Text = "Stop";
            }
            else
            {
                Application.Idle -= recordFilm;
                bRecorder.Text = "Start";
            }
            recorder = !recorder;
        }

        private void numericUpDown1_ValueChanged(object sender, EventArgs e)
        {

        }

        private void imageBoxOriginal_MouseClick(object sender, MouseEventArgs e)
        {
            numericUpDown1.Value =  e.Y;
        }
        private void tmp_undis()
        {
            //IntPtr xptr = new IntPtr();
            //IntPtr yptr = new IntPtr();
            
           Matrix<double> cam_matrix = new Matrix<double>(new Size(3, 3));
                            cam_matrix[0, 0] = 2.5531574335908931e+03;
                            cam_matrix[0, 1] = 0.0;
                            cam_matrix[0, 2] = 1.3551060874243299e+03;
                            cam_matrix[1, 0] = 0.0;
                            cam_matrix[1, 1] = 2.5545250244865060e+03;
                            cam_matrix[1, 2] = 1.0135964171368461e+03;
                            cam_matrix[2, 0] = 0.0;
                            cam_matrix[2, 1] = 0.0;
                            cam_matrix[2, 2] = 1.0;
            Matrix<double> dist_coef = new Matrix<double>(new Size(5, 1));
                            dist_coef[0,0] = 8.9580856100249545e-02;
                            dist_coef[0,1] = -2.9384181127712766e-01;
                            dist_coef[0,2] = 5.4048549797805294e-04;
                            dist_coef[0,3] = 3.4330305557660028e-03;
                            dist_coef[0,4] = 9.4612019973133649e-02;
      
            
              CvInvoke.cvInitUndistortMap(cam_matrix.Ptr, dist_coef.Ptr, x_mat.Ptr, y_mat.Ptr);
               //imgOriginal  
            CvInvoke.cvRemap(imgOriginal.Ptr,korekta.Ptr,x_mat,y_mat,(int)WARP.CV_WARP_FILL_OUTLIERS,new MCvScalar(255,0,0));
            
            
            imgOriginal = korekta.Clone();
        }
        private void readSerializedFile()
        {
            Emgu.CV.IntrinsicCameraParameters p = new IntrinsicCameraParameters();
       //     p.Undi
       /*     Matrix<double> matrix = new
            IntPtr ptr = CvInvoke.cvOpenFileStorage("parametry", IntPtr.Zero, STORAGE_OP.READ);
            XmlDocument xDoc = new XmlDocument();
            
            Image<Bgr, Byte> img = (Image<Bgr, Byte>)(new XmlSerializer(typeof(Image<Bgr, Byte>))).Deserialize(new XmlNodeReader(xDoc));
            imageBoxOriginal.Image = img;
     //       System.Runtime.Serialization.SerializationInfo info = new System.Runtime.Serialization.SerializationInfo(System.Type.DefaultBinder, new 
     //       Matrix<double> mat = new Matrix<double>(*/
        }

        private void numericUpDown2_ValueChanged(object sender, EventArgs e)
        {
         
            button1_Click(sender, e);
        }

     
    }
}
